#include "Gallery.h"
#include <cmath>
#include <complex>

Gallery::Gallery() {
}

Gallery::~Gallery() {
}

void Gallery::addFace(Face& face) {
	faces.push_back(face);
}

void Gallery::calculateOffsetMatrix() {
	// calculateAverageFace();
	
	// Allocate offset matrix
	int n = faces.size();
	int m = faces[0].getPairs().size();

	offsetMatrix = MatrixXf(m,n);

	for (int i = 0; i < n; i++) {
		vector<double> pairs = faces[i].getPairs();
		for (int j = 0; j < m; j++) {
			offsetMatrix(j,i) = pairs[j];
		}
	}

	// Fill average face with zeros
	for (int i = 0; i < m; i++) {
		averageFace.push_back(0);
	}

	// Calculate average face
	for (int i = 0; i < n; i++) {
		for (int j = 0; j < m; j++) {
			averageFace[j] += offsetMatrix(j,i);
		}
	}

	for (int i = 0; i < m; i++) {
		averageFace[i] = averageFace[i] / n;
	}

	// Calculate offset matrix
	for (int i = 0; i < n; i++) {
		for (int j = 0; j < m; j++) {
			offsetMatrix(j,i) = offsetMatrix(j,i) - averageFace[j];
		}
	}

	/*
	cout << "Average face: ";
	for (int i = 0; i < m; i++) {
		cout << averageFace[i] << " ";
	}
	cout << endl;
	vector<double> pairs = faces[0].getPairs();
	cout << "First face: ";
	for (int i = 0; i < m; i++) {
		cout << pairs[i] << " ";
	}
	cout << endl;
	
	cout << "Offset: ";
	for (int i = 0; i < m; i++) {
		cout << offsetMatrix(0, i) << " ";
	}
	cout << endl;*/
}

void Gallery::calculateCovarianceMatrix() {
	covarianceMatrix = offsetMatrix * offsetMatrix.transpose();
}

void Gallery::calculateAverageFace() {
	// Sum of all faces
	vector<Face>::iterator face;
	for (face = faces.begin(); face != faces.end(); face++) {
		vector<double> pairs = face->getPairs();
		int i = 0;
		for (vector<double>::iterator p = pairs.begin(); p != pairs.end(); p++, i++) {
			if (averageFace.size() <= i) {
				averageFace.push_back(*p);
			}
			else {
				averageFace[i] += *p;
			}
		}
	}
	
	// Calculate average face
	float numberOfFaces = faces.size();
	for (int i = 0; i < averageFace.size(); i++) {
		double pos = averageFace[i];
		averageFace[i] = pos / numberOfFaces;
	}
}

void Gallery::calculateEigenvectors() {
	SelfAdjointEigenSolver<MatrixXf> eigensolver(covarianceMatrix);
	if (eigensolver.info() == Success) {
		eigenValues = eigensolver.eigenvalues();
		eigenVectors = eigensolver.eigenvectors();
	}
}

void Gallery::compareFace(Face& f, char algorithmType, double limit) {
	
	int m = faces[0].getPairs().size();
	int n = faces.size();
	MatrixXf allFaces(n,m);
	for (int i = 0; i < faces.size(); i++) {
		MatrixXf vectorForFace = calculateVectorForFace(faces[i]);
		for (int j = 0; j < vectorForFace.rows(); j++) {
			allFaces(i,j) = vectorForFace(j);
		}
	}

	MatrixXf faceVector = calculateVectorForFace(f);
	vector<float> distances;
	float max = 0;
	double scale = pow(10.0, 10);
	for (int i = 0; i < allFaces.rows(); i++) {
		distances.push_back(0);
		for (int j = 0; j < allFaces.cols(); j++) {
			if (algorithmType == 'p') {
				distances[i] = distances[i] + pow(abs(faceVector(j) / (scale * eigenValues(j)) - allFaces(i,j) / (scale * eigenValues(j))), 2);
			}
			else if (algorithmType == 'm') {
				distances[i] = distances[i] + abs(faceVector(j) - allFaces(i,j)) / (scale * eigenValues(j));
			}
		}
		
		if (distances[i] > max) {
			max = distances[i];
		}
	}
	
	for (int i = 0; i < distances.size(); i++) {
		double value = 1 - distances[i] / max;
		if (value >= limit) {
			cout << i + 1 << ":" << 1 - distances[i] / max << endl;
		}
	}
}

MatrixXf Gallery::calculateVectorForFace(Face& f) {
	vector<double> pairs = f.getPairs();
	
	// Size of vector
	int n = pairs.size();
	
	// Create matrix of that size
	MatrixXf tmpMatrix(n,1);
	for (int i = 0; i < n; i++) {
		tmpMatrix(i) = pairs[i] - averageFace[i];
	}

	MatrixXf result = eigenVectors * tmpMatrix;
	
	return result;
}
